classdef finalSingleJoint <handle
      %默认添加重力，不要应该手动disable
      %还是以竖直开始为theta
    properties
        m   = 0.5;         % mass of robot
        n_factor;         %用于不同的n
        Ir   = 1.19e-5;        % MOI of motor ,I1自己算
        I1 = 5e-3;
        l   = 0.1;         % length of robot
        Im;   %等效Im
        g   = 9.81;        % Gravity Constant
        B1  = 2e-2;         % 阻尼系数
        Br  = 4.1e-4; 
        Bm;   %等效阻尼系数
        
        f_m = 0.5;
        f_l = 0.1;
        f_Im;
        %以顺时针为正方向
        is_g_enable;    %0 for no applying
        state;  % pos& vel & acc,三维，是减速前的，行，可以有     多行，表示不同组的输入       
        n_group = 1;
        
        joint_state;
       
    end
    
    methods
        function obj = finalSingleJoint(origin_state,is_g_enable,n_factor,m,I1,l,B1,Br)
            obj.n_group = size(origin_state,1);    
            if nargin == 8
                obj.m = m;
                obj.f_m = m;
                obj.n_factor = n_factor;
                obj.I1 = I1;
                obj.l = l;
                obj.f_l = l;
                obj.B1 = B1;
                obj.Br = Br;
                obj.is_g_enable = is_g_enable;
                obj.joint_state = origin_state;
                            obj.state = obj.joint_state./obj.n_factor;
                obj.I1 = (obj.m*obj.l^2);
            elseif nargin == 1
                obj.joint_state = origin_state;
                            obj.state = obj.joint_state./obj.n_factor;
                obj.is_g_enable = ones(obj.n_group,obj.n_group);
                obj.n_factor = repmat(10,obj.n_group,1);
                disp("only update origin state,g is enabled");
            elseif nargin == 2
                obj.joint_state = origin_state;
                            obj.state = obj.joint_state./obj.n_factor;
                obj.is_g_enable = is_g_enable;
                obj.n_factor = repmat(10,obj.n_group,1);
            elseif nargin == 3
                                obj.n_factor = n_factor;
                obj.joint_state = origin_state;
                            obj.state = obj.joint_state./obj.n_factor;
                obj.is_g_enable = is_g_enable;

            else
                ME = MException('need at least 1 param for initial state');
                throw(ME)
            end
            

            b1_factor = obj.B1./(obj.n_factor.^2);
            i1_factor = obj.I1./(obj.n_factor.^2);

            obj.Im = obj.Ir + i1_factor;
            obj.Bm = obj.Br + b1_factor;
            %之后选择在仿真那里补上I1，B1        
        end
        
        function reset_state(obj,origin_state)
            obj.joint_state = origin_state;
            obj.state = obj.joint_state./obj.n_factor;
        end

        
        function state_info = simu_loop(obj,input_tau,simu_period)                      
            tau_g = obj.m* obj.g * obj.l *obj.is_g_enable* cos(obj.joint_state(:,1));
            input_tau = input_tau + tau_g./obj.n_factor;

            %新的尝试，看是否一样
            
            obj.state(:,3) = (input_tau-obj.Bm.*obj.state(:,2))./obj.Im;
            
            obj.state(:,2) = obj.state(:,2)+ obj.state(:,3)*simu_period;
            obj.state(:,1) = obj.state(:,1)+ obj.state(:,2)*simu_period;
           
            %这些都是转子部分的
            %现在才处理关节末端的量
            obj.joint_state = obj.state./obj.n_factor;%没有太多参考意义
            state_info = obj.joint_state;     
        end
        

        function disable_g(obj,zero)
            obj.is_g_enable = zero;  
%             obj.Im = obj.Ir;
%             obj.Bm = obj.Br;
%            有无重力不影响！Im和Bm！
        end

        function tau_info = inverse_dynamic(obj,is_fake,joint_state,state)
            %注意，如果需要给逆动力学计算的控制力矩，那么应该修改state的acc
            %返回参数为1，总期望力矩，2，干扰力矩，3，线性力矩
            if(is_fake == 0)
                m  = obj.m;
                l  = obj.l;
                Im = obj.Im;
            else
                m = obj.f_m;
                l = obj.f_l;
                Im = obj.f_Im;
            end
            tau_info = zeros(obj.n_group,3);                      

            interrupt_tau = m*obj.g*l*obj.is_g_enable*cos(joint_state(:,1))./obj.n_factor;
            linear_tau  = state(:,3).*(Im) + ((obj.Bm).*state(:,2));

            desired_tau = interrupt_tau+linear_tau;
            
%             tau_info(:,1) = desired_tau;
%             tau_info(:,2) = interrupt_tau;
%             tau_info(:,3) = linear_tau;    
            
%           临时改变主意，直接传惯性力矩，阻尼力矩，重力矩绝对值了
            tau_info(:,1) = state(:,3).*(Im);
            tau_info(:,2) = ((obj.Bm).*state(:,2));
            tau_info(:,3) = interrupt_tau;
        end
        
        function update_m_l(obj,err)
            obj.f_m = err* obj.m;
            obj.f_l = err* obj.l;
            tmp = obj.f_m*obj.f_l^2;
            i1_factor = tmp./(obj.n_factor.^2);
            obj.f_Im = obj.Ir + i1_factor;
        end
        
        
    end
    
    
end